/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : mahonyAhrs.hpp
  * Description        : 
  ******************************************************************************
  * Function           :
  *
  ******************************************************************************
  * Author             : Zhiping Fu
  * Creation Date      : 2020年11月20日
  ******************************************************************************
  */
/* USER CODE END Header */

#ifndef __MAHONYAHRS_HPP
#define __MAHONYAHRS_HPP

#include "userlib/userlib.hpp"
#include "system/system.hpp"

#ifdef __cplusplus

#include <Eigen>

using namespace std;
using namespace Eigen;

class MAHONYAHRS
{
public:
	MAHONYAHRS(){}
	MAHONYAHRS(char *n) : name(n){}
	~MAHONYAHRS(){}

	void mahonyAhrs_init(void);
	void mahonyAhrs_run(void);

private:
	char *name;

	Vector3f MatAcc; //加速度计数据
	Vector3f MatGyr; //陀螺仪数据
	Vector3f MatMag; //磁力计数据
	Vector3f hn; //磁力计数据
	Vector3f bn; //磁力计数据
	Vector3f rb; //磁力计数据
	Vector3f e;
	Vector3f eInt;
	Vector3f gb;
	Vector3f w_rad;
	Matrix<float,3,3> Cb2n;
	Quaternion<float> q;  //四元数
	Vector3f Euler;  //欧拉角
	float Kp;
	float Ki;

	float OneG;       //重力 ,广州9.788m/s2
	float AngOff[3];
	bool Update;
	float Dt;

	uint32_t startTimer;
	uint32_t stopTimer;
	uint32_t  executionTime_us;

	sensor_gyro_msg gyro;
	sensor_acc_msg acc;
	sensor_mag_msg mag;
	mahonyAhrs_msg mahonyAhrs_Ang;
	Cb2n_msg MatCb2n;

};

#endif


#endif


/************************ (C) COPYRIGHT Longmen Drone Team *****END OF FILE****/
